#ifndef ROBOT_CHASSIS_CHASSIS_CONTROL_HPP
#define ROBOT_CHASSIS_CHASSIS_CONTROL_HPP

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/select.h>
#include <thread>
#include <mutex>
#include <chrono>
#include <cstdio>
#include <cstring>
#include <cmath>

enum class KeyType {
    NONE,
    UP,
    DOWN,
    LEFT,
    RIGHT,
    SPACE,
    X,
    CTRL_C,
    ESC,
    W,
    S,
    A,
    D,
    Q,
    E
};

class ChassisController : public rclcpp::Node {
public:
    ChassisController();
    ~ChassisController();
    
    void updateVelocity();
    void stop();
    void shutdown();
    void printStatus();
    
    // 按键处理
    KeyType getKey();
    void processKey(KeyType key);
    
    // 运行标志
    bool running;
    
    // 按键防抖（供外部访问）
    KeyType last_processed_key_;  // 上次处理的按键
    std::chrono::steady_clock::time_point last_processed_key_time_;  // 上次处理按键的时间
    double key_debounce_time_;  // 防抖时间间隔（秒）

private:
    void printInitInfo();
    
    // ROS2发布者
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    
    // 速度参数
    double max_linear_x_;
    double max_linear_y_;
    double max_angular_z_;
    
    // 加速度限制
    double accel_linear_;
    double accel_angular_;
    
    // 减速度限制（减速时使用，通常比加速度更大，使速度归零更快）
    double decel_linear_;
    double decel_angular_;
    
    // 目标速度
    double target_linear_x_;
    double target_linear_y_;
    double target_angular_z_;
    
    // 最高速度（记录用户设置过的最大目标速度，即使目标速度归零也保持显示）
    double max_target_linear_x_;
    double max_target_linear_y_;
    double max_target_angular_z_;
    
    // 当前速度
    double current_linear_x_;
    double current_linear_y_;
    double current_angular_z_;
    
    // 上次速度（用于检测速度变化率）
    double prev_current_linear_x_;
    double prev_current_linear_y_;
    double prev_current_angular_z_;
    
    // 控制频率
    double control_rate_;
    double dt_;
    
    // 显示相关
    double last_target_linear_x_;
    double last_target_linear_y_;
    double last_target_angular_z_;
    double last_current_linear_x_;
    double last_current_linear_y_;
    double last_current_angular_z_;
    bool first_print_;
    
    // 按键超时
    std::chrono::steady_clock::time_point last_key_press_time_;
    double key_timeout_;
    bool key_pressed_;  // 按键是否被按下
    
    // 终端设置
    struct termios original_settings_;
    int stdin_fd_;
    
    // 互斥锁
    std::mutex key_mutex_;
};

#endif // ROBOT_CHASSIS_CHASSIS_CONTROL_HPP

